/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package main;

import lejos.nxt.Motor;

/**
 *Engine for the ColorSensor
 * @author Max
 */
public class SensorEngine
{

    private Motor rotationMotor = Motor.B;
    final static int angleStep = 15;
    private int angle = 0;
    private boolean rotating = true;
    int LMR = 0;

    /**
     *
     * @param speed
     */
    public SensorEngine(int speed)
    {
        rotationMotor.setSpeed(speed);
        rotationMotor.smoothAcceleration(false);
    }

    /**
     * Rotates to angle
     * @param angle
     * @throws InterruptedException
     */
    public void rotate(int angle) throws InterruptedException
    {
        this.angle = angle;
        rotationMotor.rotateTo(this.angle, false);

    }

    /**
     * 
     * @return angle
     */
    public int getAngle()
    {

        return angle;
    }

    /**
     * Rotates left
     * @throws InterruptedException
     */
    public void rotateLeft() throws InterruptedException
    {
        if (!isLeft())
        {
            rotate(-angleStep);
        }
    }

    /**
     * Rotates right
     * @throws InterruptedException
     */
    public void rotateRight() throws InterruptedException
    {
        if (!isRight())
        {
            rotate(angleStep);
        }
    }

    /**
     * Rotates to the middle
     * @throws InterruptedException
     */
    public void rotateMiddle() throws InterruptedException
    {
        if (!isMiddle())
        {
            rotate(0);
        }
    }

    /**
     *
     * @return true if Sensor-Arm is left
     */
    public boolean isLeft()
    {
        if (getAngle() == -angleStep)
        {
            return true;
        }

        return false;
    }

    /**
     *
     * @return true if Sensor-Arm is right
     */
    public boolean isRight()
    {
        if (getAngle() == angleStep)
        {
            return true;
        }

        return false;
    }

    /**
     *
     * @return true if Sensor-Arm is in the middle
     */
    public boolean isMiddle()
    {
        if (getAngle() == 0)
        {
            return true;
        }

        return false;
    }

    /**
     * Sets if the sensor_arm shall rotate
     * @param isRotating
     */
    public void setRotating(boolean isRotating)
    {
        this.rotating = isRotating;
    }

    /**
     * Toggle Sensor one step
     * @throws InterruptedException
     */
    public void rotateColorSensor() throws InterruptedException
    {
        if (rotating)
        {
            switch (LMR)
            {
                case 0:
                    rotateMiddle();
                    break;
                case 1:
                    rotateLeft();
                    break;
                case 2:
                    rotateMiddle();
                    break;
                case 3:
                    rotateRight();
                    break;
                default:
            }
            iterateLMR();

        }
    }

    private void iterateLMR()
    {
        LMR++;

        if (LMR == 4)
        {
            LMR = 0;
        }
    }
}
   